/*
 * File: priority_path.c
 *
 * MATLAB Coder version            : 5.1
 * C/C++ source code generated on  : 02-Nov-2020 14:05:58
 */

/* Include Files */
#include <gluon_state_publisher/priority_path.h>
//#include "rt_nonfinite.h"
//#include "rt_nonfinite.h"
#include <math.h>


#define RT_PI	3.1415926535897931


bool rtIsNaN(double x)
{
    return false;
}

/* Function Definitions */
/*
 * Arguments    : int dir
 *                const double idata[48]
 *                const double last[6]
 *                double *val
 *                double *n
 * Return Type  : void
 */
void priority_path(int dir, const double idata[48], const double last[6],
                   double *val, double *n)
{
  double itheta[48];
  double weight[8];
  double count;
  int b_i;
  int i;
  int itheta_tmp;
  bool exitg1;
  bool guard1 = false;
  memset(&itheta[0], 0, 48U * sizeof(double));
  count = 0.0;

  /* ����?�??���?����??��   */
  for (i = 0; i < 8; i++) {
    weight[i] = 0.0;
    guard1 = false;
    if (dir > 0.0) {
      /* ?�?�������������?�  */
      if (!(idata[i] < 0.0)) {
        guard1 = true;
      }
    } else if ((!(dir < 0.0)) || (!(idata[i] > 0.0))) {
      /* ��???�??��� */
      guard1 = true;
    } else {
      /* ?�?�������������?� */
    }

    if (guard1) {
      for (b_i = 0; b_i < 6; b_i++) {
        itheta_tmp = i + (b_i << 3);
        itheta[itheta_tmp] = idata[itheta_tmp];
      }

      count++;
    }
  }

  /*      disp('================================================================='); */
  /*      disp(itheta*180/pi); */
  /* �?������?������?����?�?�    */
  if (count > 0.0) {
    for (i = 0; i < 8; i++) {
      if ((itheta[i] == 0.0) && (itheta[i + 8] == 0.0) && (itheta[i + 16] == 0.0)
          && (itheta[i + 24] == 0.0) && (itheta[i + 32] == 0.0) && (itheta[i +
           40] == 0.0)) {
        weight[i] = 37.699111843077517;
      } else {
        /*              disp(tmp); */
        count = weight[i];
        for (itheta_tmp = 0; itheta_tmp < 6; itheta_tmp++) {
          count += fabs(itheta[i + (itheta_tmp << 3)] - last[itheta_tmp]);
        }

        weight[i] = count;
      }
    }

    /*          disp('?�?�'); */
    /*          disp(weight) */
    if (!rtIsNaN(weight[0])) {
      itheta_tmp = 1;
    } else {
      itheta_tmp = 0;
      i = 2;
      exitg1 = false;
      while ((!exitg1) && (i < 9)) {
        if (!rtIsNaN(weight[i - 1])) {
          itheta_tmp = i;
          exitg1 = true;
        } else {
          i++;
        }
      }
    }

    if (itheta_tmp == 0) {
      *val = weight[0];
      itheta_tmp = 1;
    } else {
      *val = weight[itheta_tmp - 1];
      b_i = itheta_tmp + 1;
      for (i = b_i; i < 9; i++) {
        count = weight[i - 1];
        if (*val > count) {
          *val = count;
          itheta_tmp = i;
        }
      }
    }

    *n = itheta_tmp;

    /*          disp('���?�'); */
    /*          disp(val); */
    /*          disp('���??'); */
    /*          disp(n); */
    /*          disp(idata(n,:)*180/pi); */
  } else {
    *val = 0.0;
    *n = 0.0;

    /*          disp('?�?��??�'); */
  }

  /*      disp('-----------------------------------------------------------------'); */
}

/*
 * File trailer for priority_path.c
 *
 * [EOF]
 */
